package sma.iss;

import java.io.IOException;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.Iterator;
import java.util.LinkedList;
import java.util.List;
import java.util.Random;
import java.util.Set;
import java.util.SortedSet;
import java.util.TreeSet;

import sma.iss.Position.Origin;
import sma.iss.calc.PathOptimizer;
import sma.iss.sort.PureDistanceComparator;
import sma.iss.sort.ScoreComparator;
import sma.iss.sort.ScoreCumulComparator;
import sma.iss.sort.ScoreDistanceComparator;
import sma.iss.util.AngleUtil;
import sma.nasa.ConstraintsChecker;

/**
 * Several turn of ISS to find the good way. For one beta angle
 * 
 * @author smarsoll
 * 
 */
public class GeneticModel extends DynamicModel {

	/** speeds are random between -0.5 to +0.5 by: */
	double maxMutantSpeedSegment = ConstraintsChecker.MAX_SARJ_SPEED;
	protected Random rnd = new Random();
	protected int currentGen = 0;
	/** nb of generations in genetic optimisation, by default 3 */
	public int nbGeneration = 3;
	public int nbKeepForNextTurn = 1;

	public double averageScore = Double.NEGATIVE_INFINITY;
	/** for quick calculation */
	public boolean improveMandatory = true;
	/** stop caculating if no improvement since nb generation. */
	public int nbNoImproveMaxPerTurn = 4;

	GeneticModel() {
		super();
		// to force not use this variable
		this.positions = null;
	}
	/**
	 * 
	 * @param minute
	 * @return
	 */
	public Collection<Position> calculateListOfPosition(int minute) {
		LOGGER.info("=========================================== calculatePosition minute={}  beta={}", minute, beta);
		if (mapListPos.isEmpty()) {
			// init with static model
			Set<Position> initPos = Collections.singleton(createInitialPosition(minute));
			mapListPos.put(minute, initPos);
			return initPos;
		}
		// a way access last position and determine deltaMinute : nextMinute contain last minute calculated
		int minBefore = minuteToCalc;
		minuteToCalc = minute;
		deltaMinute = minute - minBefore;
		deltaSecCheck = deltaMinute * 60.0;
		// deltaSeconde = deltaMinute * 60.0 * TIME_REDUCER;
		deltaSeconde = deltaMinute * 60.0;

		Collection<Position> bestPosOfThisTurn = new TreeSet<Position>(new ScoreCumulComparator());
		// For each of the last turn positions kept
		Collection<Position> listPreviousPos = mapListPos.get(minBefore);
		for (Position previousPos : listPreviousPos) {
			lastPos = previousPos;
			// any target
			if (minuteToCalc > (92 - 25)) {
				targetPos = extractParent(previousPos, 0);
			} else {
				targetPos = null;
			}

			// optimise to find the best positions
			Collection<Position> currentPosBestResults = optimiseNextPositionGenetic();
			bestPosOfThisTurn.addAll(currentPosBestResults);

			LOGGER.info("for minute {} , {}", minuteToCalc, currentPosBestResults);
		}

		Collection<Position> nextTurnList = new LinkedList<Position>();
		Collection<Position> bestPosOfThisTurn2 = new TreeSet<Position>(new PureDistanceComparator());

		Iterator<Position> iterator = bestPosOfThisTurn.iterator();
		Position theBest = iterator.next();
		nextTurnList.add(theBest);
		int nbKept = 1;
		iterator.remove();
		while (iterator.hasNext()) {
			Position pos1 = iterator.next();
			// limite influence of distance with a logarythm
			pos1.tempDistance = pos1.getScore() + Math.log10(theBest.distance(pos1));
			// pos1.tempDistance = Math.sqrt(theBest.distance(pos1));
			bestPosOfThisTurn2.add(pos1);
		}

		Iterator<Position> it = bestPosOfThisTurn2.iterator();

		while (it.hasNext() && nbKept < nbKeepForNextTurn) {
			Position bestForNextTurn = it.next();
			bestForNextTurn.setMinute(minuteToCalc);
			nextTurnList.add(bestForNextTurn);
			LOGGER.info("distance with best = {}", theBest.distance(bestForNextTurn));
			nbKept++;
		}

		// store next position
		// nextPos.minute = minute;
		// positions.put(minute, nextPos);
		mapListPos.put(minute, nextTurnList);
		LOGGER.debug("for turn minute {} , {}", minuteToCalc, nextTurnList);
		return nextTurnList;
	}

	private Collection<Position> optimiseNextPositionGenetic() {
		LOGGER.debug("optimiseNextPositionGenetic");
		initBeforeMinuteCalculation();
		currentNbAdcance = nbAdvance;
		currentGen = 0;
		if (nextPos == null) {
			// the new position to optimise
			nextPos = new Position(lastPos, Origin.PROJECTION, currentGen);
			nextPos.setMinute(minuteToCalc);
			// init from last state considering the delta
			nextPos.initState(lastPos, deltaMinute, currentNbAdcance - 1);

		}
		LOGGER.info("Starting from {}", nextPos);


		Collection<Position> population = new ArrayList<Position>();
		// Fill initial population
		addAdvanceChild(nextPos);
		// normal position
		population.add(nextPos);

		if (!enhanceMode) {
			// all max and min speeds
			for (int numRot = 0; numRot < NB_ROTERS; numRot++) {
				Position minMax;
				double maxSpeed = getMaxSpeed(numRot);
				double maxAcc = getMaxAcc(numRot);
				minMax = createNewExtremPosition(numRot, maxSpeed, maxAcc);
				population.add(minMax);
				minMax = createNewExtremPosition(numRot, maxSpeed, -maxAcc);
				population.add(minMax);
			}
		}

		{
			// case where 8 BGA did not rote
			Position interPos = new Position(nextPos, Origin.ZERO, currentGen);
			for (int iBga = 0; iBga < 8; iBga++) {
				int numRotZero = iBga + 2;
				interPos.setAngle(numRotZero, lastPos.getAngle(numRotZero));
				interPos.setSpeed(numRotZero, 0.0);
			}
			addAdvanceChild(interPos);
			population.add(interPos);
		}

		// the path to rendez-vous of next revolution
		if (targetPos != null) {

			LOGGER.debug("check rendez-vous");
			// Position pos0 = positions.get(0);
			Position target = targetPos;

			// if (nextMinute == 84 && numRot == 1) {
			// LOGGER.info("debug");
			// }

			// LOGGER.info("Can not optimise, need to interpolate to rendez-vous mn={} rot={}", minuteToCalc, numRot);
			Position posBefore = lastPos;


			Position interPos = createInterpolation(target, posBefore, minuteToCalc);
			addAdvanceChild(interPos);
			population.add(interPos);

		}

		// some random elements
		if (enhanceMode) {
			population.addAll(generateRandomPositions(15));
		} else {
			population.addAll(generateRandomPositions(10));
		}

		// Some generation 0 children
		// population.addAll(addChildren(population, 18));
		double currentBest = -1;
		double lastBest = -1;
		int nbNoImprove = 0;

		// for each new generation wanted
		for (int i = 1; i <= nbGeneration; i++) {
			currentGen = i;
			if (i != 1) {
				Iterator<Position> iterator = population.iterator();
				Position bestScorePos = iterator.next();
				Position secondScorePos = iterator.next();
				List<Position> plusBeaux = new ArrayList<Position>(population);
				plusBeaux.add(secondScorePos);
				population.addAll(addChildren(bestScorePos, plusBeaux, 8));

				// children between 1st/3rd.
				List<Position> subList = new ArrayList<Position>(population);
				subList = subList.subList(0, Math.min(subList.size() - 1, population.size() / 3));
				population.addAll(addChildren(subList, 12));
				population.addAll(addMutant(subList, 5));
			}
			// children between all
			population.addAll(addChildren(population, 7));
			if (i != nbGeneration) {
				population.addAll(addMutant(population, 7));
			}

			// check transition is possible, remove bad
			removeImpossibleTransitions(population, false);
			calculateScores(population);

			// new population is sorted by score (or score * distance best)
			population = cleanPopulation(population);
			currentBest = population.iterator().next().getScore();
			// if (improveMandatory) {
				if (currentBest == lastBest) {
					nbNoImprove++;
					if (nbNoImprove >= nbNoImproveMaxPerTurn) {
						LOGGER.info("stoping loop {} because score {} was not improved during {} turns", i, currentBest,
								nbNoImproveMaxPerTurn);
						break;
					}
			} else {
				nbNoImprove = 0;
				}
			// }
			lastBest = currentBest;
		}

		// keep the best scores
		Collection<Position> result = new LinkedList<Position>();
		{
			removeImpossibleTransitions(population, true);
			Position best = population.iterator().next();
			population.remove(best);
			result.add(best);
			result.addAll(extractScoreDistance(best, population, 3));
		}
		// cumulate scores
		for (Position position : result) {
			position.cumulateScore();
		}

		// nextPos = keepBestScores(population);
		nextPos = null;
		return result;
	}

	private Position createInterpolation(Position target, Position posBefore, int minuteInter) {
		Position interPos = new Position(nextPos, Origin.INTERPOL, currentGen);
		interPos.setMinute(minuteInter);
		int deltaTotalMn = posBefore.getDelatMinuteToPos(target);
		int deltaInterMn = posBefore.getDelatMinuteToPos(interPos);
		double[] angleAndSpeed;
		for (int numRot = 0; numRot < 10; numRot++) {
			PathOptimizer opt = PathOptimizer.getPathOptimizer(numRot);

			angleAndSpeed = opt.interpolate(posBefore.getAngle(numRot), posBefore.getSpeed(numRot), target.getAngle(numRot),
					target.getSpeed(numRot), deltaTotalMn * 60.0, deltaInterMn * 60.0);
			interPos.setAngle(numRot, angleAndSpeed[0]);
			interPos.setSpeed(numRot, angleAndSpeed[1]);
			interPos.setInterpolated(numRot, true);

		}
		return interPos;
	}

	protected Collection<Position> extractScoreDistance(Position theBest, Collection<Position> population, int nbToKeep) {
		if (population.isEmpty()) {
			return new LinkedList<Position>();
		}
		// now considere the distance
		SortedSet<Position> distanceSorted = new TreeSet<Position>(new ScoreDistanceComparator());
		Iterator<Position> it = population.iterator();
		while (it.hasNext()) {
			Position toCompare = it.next();
			if (toCompare.getScore() > 0) {
				toCompare.tempDistance = theBest.distance(toCompare);
				// LOGGER.info("distance={} for {}", toCompare.tempDistance, toCompare);
				distanceSorted.add(toCompare);
			}
		}
		List<Position> list = new ArrayList<Position>(distanceSorted);
		return list.subList(0, Math.min(list.size() - 1, nbToKeep));
	}

	protected Collection<Position> generateRandomPositions(int nbRandom) {
		Collection<Position> result = new ArrayList<Position>(nbRandom);
		for (int i = 0; i < nbRandom; i++) {
			Position parent = lastPos;
			Position minMax = generateOneRandomPosition(parent);
			result.add(minMax);
			addAdvanceChild(minMax);
		}
		return result;
	}

	/** add random children */
	private void addAdvanceChild(Position parent) {
		int nbChildren = currentNbAdcance - 1;
		for (int j = 0; j < nbChildren; j++) {
			Position minMax = generateOneRandomPosition(parent);
			parent.childAdvance = minMax;
			// for next
			parent = minMax;
		}
	}

	protected Position generateOneRandomPosition(Position parent) {
		Position minMax = new Position(nextPos, Origin.RANDOM, currentGen);
		for (int numRot = 0; numRot < NB_ROTERS; numRot++) {
			double targetSpeed = getMaxSpeed(numRot) * (2.0 * rnd.nextDouble() - 1.0);
			double targetAcc = getMaxAcc(numRot) * (2.0 * rnd.nextDouble() - 1.0);
			double[] nextShiftSpeed = calculateNextPos(parent.getSpeed(numRot), targetSpeed, targetAcc, deltaSeconde);
			minMax.setAngle(numRot, AngleUtil.addAngle(parent.getAngle(numRot), nextShiftSpeed[0]));
			minMax.setSpeed(numRot, nextShiftSpeed[1]);
		}
		return minMax;
	}

	/**
	 * 
	 * @param population
	 * @return best in first position
	 */
	protected Collection<Position> cleanPopulation(Collection<Position> population) {
		int startSize = population.size();
		LOGGER.debug("cleanPopulation start={}", startSize);
		Collection<Position> result = new ArrayList<Position>(population.size());
		ScoreComparator comp = new ScoreComparator();
		// sort by score
		SortedSet<Position> sorted = new TreeSet<Position>(comp);
		sorted.addAll(population);
		Iterator<Position> it = sorted.iterator();
		// keep the best
		Position theBest = it.next();
		result.add(theBest);
		int nbTaken = 1;
		// keep best positions
		int nbToKeep = population.size() / 8;
		for (int i = 0; i < nbToKeep; i++) {
			result.add(it.next());
			nbTaken++;
		}
		Iterator<Position> itDist1;
		{
			// now considere the distance
			SortedSet<Position> distanceSorted = new TreeSet<Position>(new PureDistanceComparator());
			nbToKeep = population.size() / 4;
			while (it.hasNext()) {
				Position toCompare = it.next();
				if (toCompare.getScore() > 0) {
					toCompare.tempDistance = +toCompare.getScore() + Math.log10(theBest.distance(toCompare));
					// LOGGER.info("distance={} for {}", toCompare.tempDistance, toCompare);
					distanceSorted.add(toCompare);
				}
			}
			it = null;
			// now consider distance to best score
			itDist1 = distanceSorted.iterator();
			while (itDist1.hasNext() && nbTaken <= nbToKeep) {
				result.add(itDist1.next());
				nbTaken++;
			}
		}

		{
			// now considere the distance 2 times
			SortedSet<Position> distanceSorted = new TreeSet<Position>(new ScoreDistanceComparator());
			nbToKeep = Math.max(population.size() / 2, 18);
			while (itDist1.hasNext()) {
				Position toCompare = itDist1.next();
				if (toCompare.getScore() > 0) {
					// toCompare.tempDistance = theBest.distance(toCompare);
					toCompare.tempDistance = toCompare.tempDistance * toCompare.tempDistance;
					// LOGGER.info("distance={} for {}", toCompare.tempDistance, toCompare);
					distanceSorted.add(toCompare);
				}
			}
			// now consider distance to best score
			Iterator<Position> itDist2 = distanceSorted.iterator();
			while (itDist2.hasNext() && nbTaken <= nbToKeep) {
				result.add(itDist2.next());
				nbTaken++;
			}
		}

		LOGGER.info("cleanPopulation from {} to {} {}", startSize, result.size(), result);
		return result;
	}

	protected Collection<Position> addMutant(Collection<Position> population, int nbMutant) {
		Collection<Position> result = new ArrayList<Position>(nbMutant);
		List<Position> melange = shufflePopulation(population, nbMutant);
		int nbAdded = 0;
		for (Position parent : melange) {
			Position interPos = new Position(parent, Origin.MUTANT, currentGen);
			boolean doMute = true;
			while (doMute) {
				int numRot = Math.abs(rnd.nextInt()) % 10;
				PathOptimizer opt = PathOptimizer.getPathOptimizer(numRot);
				double targetSpeed = maxMutantSpeedSegment * (2.0 * rnd.nextDouble() - 1.0);
				double shift = opt.calculateSimpleShift(lastPos.getSpeed(numRot), targetSpeed, deltaSeconde);
				interPos.setAngle(numRot, AngleUtil.addAngle(lastPos.getAngle(numRot), shift));
				interPos.setSpeed(numRot, targetSpeed);
				doMute = rnd.nextBoolean();
			}
			result.add(interPos);
			nbAdded++;
			if (nbAdded >= nbMutant) {
				break;
			}
		}
		LOGGER.debug("nb mutant generated={}", result.size());
		return result;
	}

	protected Collection<Position> addChildren(Collection<Position> population, int nbChildren) {
		Collection<Position> newChildren = new ArrayList<Position>(nbChildren);
		List<Position> melange = shufflePopulation(population, nbChildren * 2);

		Iterator<Position> itParent = melange.iterator();

		for (int j = 0; j < nbChildren; j++) {
			Position p1 = itParent.next();
			Position p2 = itParent.next();
			Position child = new Position(nextPos, Origin.CHILD, currentGen);
			fillChildWithParents(child, p1, p2);
			// child.
			newChildren.add(child);
		}
		return newChildren;
	}

	protected Collection<Position> addChildren(Position mainParent, Collection<Position> population, int nbChildren) {
		Collection<Position> newChildren = new ArrayList<Position>(nbChildren);
		List<Position> melange = shufflePopulation(population, nbChildren);

		Iterator<Position> itParent = melange.iterator();

		for (int j = 0; j < nbChildren; j++) {
			Position p1 = mainParent;
			Position p2 = itParent.next();
			if (p1 != p2) {
				Position child = new Position(nextPos, Origin.CHILD, currentGen);
				fillChildWithParents(child, p1, p2);
				// child.
				newChildren.add(child);
			}
		}
		return newChildren;
	}

	protected List<Position> shufflePopulation(Collection<Position> population, int minSize) {
		List<Position> melange = new ArrayList<Position>(minSize * 2);
		while (melange.size() < minSize) {
			List<Position> melange1 = new ArrayList<Position>(population);
			Collections.shuffle(melange1, rnd);
			melange.addAll(melange1);
		}
		return melange;
	}


	protected void fillChildWithParents(Position child, Position p1, Position p2) {
		for (int numRot = 0; numRot < 10; numRot++) {
			double coef1 = rnd.nextDouble();
			double coef2 = 1.0 - coef1;
			double angle = AngleUtil.reduce(AngleUtil.getShift(0, p1.getAngle(numRot)) * coef1
					+ AngleUtil.getShift(0, p2.getAngle(numRot)) * coef2);
			child.setAngle(numRot, angle);
			child.setSpeed(numRot, p1.getSpeed(numRot) * coef1 + p2.getSpeed(numRot) * coef2);
		}
	}

	private Position createNewExtremPosition(int numRot, double maxSpeed, double maxAcc) {
		double[] nextShiftSpeed;
		Position minimPos = new Position(nextPos, Origin.MAX, currentGen);
		nextShiftSpeed = calculateNextPos(lastPos.getSpeed(numRot), maxSpeed, maxAcc, deltaSeconde);
		minimPos.setAngle(numRot, AngleUtil.addAngle(lastPos.getAngle(numRot), nextShiftSpeed[0]));
		Position parent = minimPos;
		minimPos.setSpeed(numRot, nextShiftSpeed[1]);

		if (currentNbAdcance > 1) {
			Position child = new Position();
			minimPos.childAdvance = child;
			child.origin = Origin.CHILDMAX;
			child.setMinute(minimPos.getMinute() + deltaMinute);
			// -2 because one child is already created
			child.initState(minimPos, deltaMinute, currentNbAdcance - 2);
		}
		//
		// for (int i = 1; i < currentNbAdcance; i++) {
		// Position childPos = new Position(azeze, Origin.MAX, currentGen);
		// parent.childAdvance = childPos;
		// nextShiftSpeed = calculateNextPos(lastPos.getSpeed(numRot), maxSpeed, maxAcc, deltaSeconde);
		// childPos.setAngle(numRot, AngleUtil.addAngle(lastPos.getAngle(numRot), nextShiftSpeed[0]));
		// childPos.setSpeed(numRot, nextShiftSpeed[1]);
		// }
		LOGGER.debug("minimPos rot {} speed {} {}", numRot, minimPos.getSpeed(numRot), minimPos);
		return minimPos;
	}

	public void enhanceOneNewPosition(int minute) {
		LOGGER.info("============ enhancing minute {}  beta {} yaw {}", minute, beta, yaw);
		minuteToCalc = minute;
		// pos before
		int minBefore = -7000;
		for (int i = minute - 1; i > minute - 92; i--) {
			minBefore = i;
			int index = i;
			if (index < 0) {
				index += 92;
			}
			lastPos = positions.get(index);
			if (lastPos != null) {
				break;
			}
		}

		deltaMinute = minute - minBefore;
		deltaSecCheck = deltaMinute * 60.0;
		// deltaSeconde = deltaMinute * 60.0 * TIME_REDUCER;
		deltaSeconde = deltaMinute * 60.0;

		// pos after
		for (int i = minute + 1; i < minute + 92; i++) {
			int index = i;
			if (index >= 92) {
				index -= 92;
			}
			targetPos = positions.get(index);
			if (targetPos != null) {
				break;
			}
		}
		// we may have already a position to enhance
		Position existing = positions.get(minute);
		if (existing != null) {
			nextPos = new Position(existing, Origin.CSV, -1);
			// assume existing transition was valid.
			nextPos.transitionOk = true;
			calculateScore(issCalculator, nextPos);
		}
		Collection<Position> currentPosBestResults = optimiseNextPositionGenetic();
		Position enhancePos = currentPosBestResults.iterator().next();
		LOGGER.info("for minute {} , {}", minute, enhancePos);
		positions.put(minute, enhancePos);

		// update parent relation:
		targetPos.parent = enhancePos;
	}

	public int enhanceInterleavePosition() throws IOException {
		LOGGER.info("existing positions = {}", positions);
		int nbAdded = 0;
		// ArrayList<Position> lstPos = new ArrayList<Position>(positions.values());
		ArrayList<Integer> lstPos = new ArrayList<Integer>(positions.keySet());
		// add 92 which is in fact 0
		lstPos.add(92);
		LOGGER.info("lstPos = {}", lstPos);
		Iterator<Integer> it = lstPos.iterator();
		int previous = it.next();
		while (it.hasNext()) {
			int after = it.next();
			if (after - previous > 1) {
				// we can put a new position
				int newMinute = ((after - previous) / 2) + previous;
				LOGGER.info("=========================== new minute Interleave = {}  for beta", newMinute, beta);
				enhanceOneNewPosition(newMinute);
				nbAdded++;
				// problem with interpolation
				// this.saveResultInCsv("intermediate save for mn=" + newMinute);
				// this.saveSimulation();
			}
			// for next turn
			previous = after;
		}
		return nbAdded;
	}

	public void enhanceExistingPosition() throws IOException {
		LOGGER.info("existing positions = {}", positions);
		// ArrayList<Position> lstPos = new ArrayList<Position>(positions.values());
		ArrayList<Integer> lstPos = new ArrayList<Integer>(positions.keySet());
		// add 92 which is in fact 0
		for (Integer minute : lstPos) {
			enhanceOneNewPosition(minute);
			// problem with interpolation
			// this.saveResultInCsv("intermediate save for mn=" + minute);
		}

	}

	public void iterateOnMinutes(int mnPerStep) {
		Collection<Position> lastList = null;
		lastList = mapListPos.get(0);
		for (int minute = mnPerStep; minute < 92; minute += mnPerStep) {
			if (minute <= 92 - (mnPerStep / 2)) {
				lastList = this.calculateListOfPosition(minute);
			} else {
				LOGGER.info("Mn {} is too much close to minute 92/0", minute);
			}

		}
		this.fillMapMinutes(lastList.iterator().next());
	}

	public void calculateAverageScore() {
		int nbAdded = 0;
		averageScore = 0.0;
		for (Position pos : positions.values()) {
			if (pos.getMinute() >= 0 && pos.getMinute() < 92) {
				averageScore += pos.getScore();
				nbAdded++;
			}
		}
		averageScore = averageScore / nbAdded;
		LOGGER.info("averageScore={}", averageScore);
	}

	@Override
	public String toString() {
		return "GeneticModel [avSc=" + averageScore + ", yaw=" + yaw + ", beta=" + beta + "]";
	}

	public int nbCaculationDone() {
		return nbCaculationDone.get();
	}

}
